using System;
using System.Collections.Generic;
using System.Text;
using System.IO;
using DotNetMatrix;


namespace Blimpbots.Recognition.Algorithms
{
    public class KalmanFilter
    {
        protected GeneralMatrix _x;
        protected GeneralMatrix _P;
        protected GeneralMatrix _F;
        protected GeneralMatrix _Q;
        protected GeneralMatrix _H;
        protected GeneralMatrix _R;

        public GeneralMatrix Predict()
        {
            _x = _F * _x; //Could add control term here
            _P = (_F * _P) * _F.Transpose() + _Q;
            return _x;
        }

        public GeneralMatrix PredictedP;

        public GeneralMatrix PredictNonDestructive()
        {
            GeneralMatrix x = _F * _x; //Could add control term here
            PredictedP = (_F * _P) * _F.Transpose() + _Q;
            return x;
        }

        public GeneralMatrix Update(GeneralMatrix z)
        {
            GeneralMatrix I = GeneralMatrix.Identity(_x.RowDimension, _x.RowDimension);
            GeneralMatrix y = new GeneralMatrix(z.RowDimension, z.ColumnDimension);
            GeneralMatrix S = new GeneralMatrix(_H.RowDimension, _H.ColumnDimension);
            GeneralMatrix K = new GeneralMatrix(_P.RowDimension, _P.ColumnDimension);

            GeneralMatrix hx = _H * _x;
            y = z.Subtract(hx);
            S = (_H *_P * (_H.Transpose())) + _R;
            K = (_P * (_H.Transpose())) * S.Inverse();
            _x = _x + (K * y);
            _P = (I.Subtract(K * _H)) * _P;

            return _x;
        }
    }
}
